/*
*  This file is part of ygg-brute
*  Copyright (c) 2020 ygg-brute authors
*  See LICENSE for licensing information
*/

DECLSPEC inline void node_id_to_ipv6(const uint8_t node_id[64], uint8_t ipv6[16])
{
    #define GET_UINT32(a, i) (uint32_t(a[i + 3]) | (uint32_t(a[i + 2]) << 8) | (uint32_t(a[i + 1]) << 16) | (uint32_t(a[i]) << 24))

    uint16_t cnt = 0;

    for(unsigned i = 0; i < 64; ++i) {
        uint8_t c = node_id[i];
        if(c != 0xff) {
            while(c & 0x80) {
                ++cnt;
                c <<= 1;
            }
            break;
        }
        cnt += 8;
    }

    ipv6[0] = 0x2;
    ipv6[1] = cnt > 0xff ? 0xff : cnt; // improbable

    unsigned idx = (cnt + 1) / 8;
    unsigned shift = 8 - ((cnt + 1) % 8);
    uint32_t cur = node_id[idx];

    // TODO : handle very improbable out-of-range situation
    for(unsigned i = 0; i < 14; ++i) {
        cur <<= 8;
        cur |= (uint8_t)node_id[idx + i + 1];
        ipv6[i + 2] = cur >> shift;
    }

    #undef GET_UINT32
}